Field of the Invention
The present invention relates to a technique for measuring a position and orientation of an object.
Description of the Related Art
In recent years, robots are gradually handling complicated tasks such as assembling industrial products in place of human beings. In assembling of industrial products by robots, a relative position and orientation between components has to be precisely measured so as to grip a component by an end effector such as a robot hand. As a method of measuring a position and orientation, measurements by model fitting, which fits a three-dimensional shape model to features detected from a grayscale image captured by a camera or a range image obtained from a range sensor are generally used. Of such methods, non-patent literature 1 (Christopher Kemp, Tom Drummond, “Dynamic Measurement Clustering to Aid Real Time Tracking,” iccv, vol. 2, pp. 1500-1507, Tenth IEEE International Conference on Computer Vision (ICCV'05) Volume 2, 2005.) discloses a method of estimating a position and orientation at high speed by efficiently selecting features (three-dimensional points, lines, planes, and the like; to be referred to as geometric features hereinafter) in a three-dimensional shape model. The method disclosed in non-patent literature 1 groups geometric features in a three-dimensional shape model into geometric feature groups which have similar influences on parameters that express a position and orientation to be estimated. Then, a position and orientation is estimated based on a small number of geometric features selected from the respective groups. This method calculates which of six degrees of freedom parameters, that together express a position and orientation, a geometric feature readily influences. The calculation is performed by calculating, for each of the geometric features, a change amount (to be referred to as a Jacobian hereinafter) of that geometric feature when each of the six parameters of the degrees of freedom of the position and orientation is minimally changed.
However, the method according to non-patent literature 1 suffers the following problem. That is, since grouping processing of geometric features is executed during model fitting processing, processing cost of the grouping processing of geometric features increases depending on the number of geometric features and grouping methods, thus impairing the speed of the model fitting processing.